#include "ratecontrol.h"

#define MAX_SAMPLE 5

typedef enum {
    false = 0,
    true  = 1
} bool;

float	m_bit_rate;
float   m_PreviousBit_Rate;
float	m_rcp_frame_rate;
float	m_frame_rate;
float	m_GAMMAP;
float	m_BETAP;
int		m_Frame_Total_Number_MB;
int		m_TotalQpforPPicture;
int		m_NumberofPPicture;

int		m_NumberofCodedBFrame;
int		m_NumberofCodedPFrame;
int		m_NumberofGOP;

int		m_RC_MAX_QUANT=51;
int		m_RC_MIN_QUANT=0;

float	m_BufferSize;
int		m_GOPTargetBufferLevel;
int		m_CurrentBufferFullness;
int		m_TargetBufferLevel;
int		m_MyInitialQp;
int		m_PAverageQp;

// coefficients of the prediction model
float	m_PreviousPictureSATD;
float	m_SATDPictureC1;
float	m_SATDPictureC2;
float	m_PSATDPictureC1;
float	m_PSATDPictureC2;

float	m_PPictureSATD[ MAX_SAMPLE + 1 ];
float	m_PictureSATD[ MAX_SAMPLE + 1 ];
float	m_ReferenceSATD[ MAX_SAMPLE + 1 ];

// quadratic rate-distortion model
float	m_rgQp[ MAX_SAMPLE + 1 ];
float	m_rgRp[ MAX_SAMPLE + 1 ];
float	m_rcp_m_rgQp[ MAX_SAMPLE + 1 ];			// 1.0f / m_rgQp[ i ]
float	m_rcp_m_rgQp_m_rgQp[ MAX_SAMPLE + 1 ];	// 1.0f / ( m_rgQp[ i ] * m_rgQp[ i ] )
float	m_m_rgQp_m_rgRp[ MAX_SAMPLE + 1 ];		// ( m_rgQp[ i ] * m_rgRp[ i ] )
float	m_X1;
float	m_X2;
int		m_Qc;
float	m_Qstep;
int		m_Qp;
int		m_Pm_Qp;
int		m_PreviousQp1;
int		m_PreviousQp2;

float	m_MINVALUE;
float	m_CurrentFrameSATD;		// Richard add for rate control 02-22-2006
float	m_PreviousFrameSATD;	// Richard add for rate control 02-22-2006
int		m_windowSize;
int		m_SATDm_windowSize;
int		DDquant;
int		QPLastPFrame;
int		QPLastGOP;

float	m_Pm_X1;
float	m_Pm_X2;

bool	m_GOPOverdue;

int		m_R;
int		m_Np;
int		m_T, m_T1;

// HRD consideration
int		m_UpperBound1, m_UpperBound2, m_LowerBound;
float	m_InitialDelayOffset;
float	m_OMEGA = 0.9f;

int		m_TotalPFrame;
int		m_DuantQp; 
int		m_PDuantQp;
int		m_DeltaP;

float	RC_a00,   RC_a01,   RC_a10,   RC_a11,   RC_b0,   RC_b1;
float	SATD_a00, SATD_a01, SATD_a10, SATD_a11, SATD_b0, SATD_b1;

int 	rtcl_offset;

int     m_frame_cost=0;

#define limited_RC_Min_Quant 16
#define max_frame_rate 60
int satd_array[max_frame_rate];
#define check_range 1

int check_satd_distribution(int satd);
#if 0

#define TOTAL_FRAME_NUMBER   300
#define FRAME_PERIOD		 30
#define PFRAME_NUMBER		 29
#define BITS				 3421		// total bit count
#define CURREN_FRAME_SATD	 1208

// ---------------------------------------
// the main function is just for testing
// and will not be integrated.
// ---------------------------------------
int 
main( void )
{
	unsigned int i, img_type, END = 0;


	SEQUENCE _seq;
	_seq.jumpd      = 0;
	_seq.bit_rate   = 256000;
	_seq.frame_rate = 30.0;
	_seq.width		= 176;
	_seq.height		= 144; 
	_seq.initialQP	= 0;

	rc_init_seq( &_seq );

	for( i = 0 ; i < TOTAL_FRAME_NUMBER ; ++i )
	{
		if( ( i % FRAME_PERIOD ) == 0 )	// I-frame
		{
			img_type = I_SLICE;
			rc_init_GOP( 29, &_seq );
		}
		else							// P-frame
		{
			// begin to encode one frame
			img_type = P_SLICE;
		}

		rc_init_pict( img_type, _seq.bit_rate ); // for VBR case, update the bit rate per frame.
		updateQuantizationParameter( img_type );

		// ENCODE A FRAME HERE......
		// when encode finish, we have :
		// 1) total bit count
		// 2) current frame SATD

		rc_update_pict_frame( img_type );
		rc_update_pict( BITS );
		updateRCModel( CURREN_FRAME_SATD, img_type, BITS * 0.6 );
	}
	END = 1;
}

#endif


//----------------------------------------
//			rc_init_seq
//	initialize parameter per-sequence
//	Richard	03/07/2006 
//
//----------------------------------------
void 
rc_init_seq( SEQUENCE* seq )
{
  float L1, L2, L3, bpp;
  int qp;
  int i;
  
  m_bit_rate	     = seq->bit_rate;
 
  m_PreviousBit_Rate = m_bit_rate;					 // Richard 05-18-2006
  m_frame_rate	     = seq->frame_rate / (float)(seq->jumpd + 1);
  m_rcp_frame_rate   = 1.0f / m_frame_rate;
   
  m_Frame_Total_Number_MB = seq->height * seq->width / 256;
  rtcl_offset = 2*256*m_Frame_Total_Number_MB;
  
  // initialize the parameters of fluid flow traffic model
  //m_BufferSize				= m_bit_rate * 1.0f;	//2.56;
  m_BufferSize				= m_bit_rate * 10.0f;	//2.56;
  m_CurrentBufferFullness	= 0;
  m_GOPTargetBufferLevel	= m_CurrentBufferFullness;
  // HRD consideration
  m_InitialDelayOffset		= m_BufferSize * 0.8f;
  
  // initialize the previous window size
  m_windowSize			= 0;
  m_SATDm_windowSize	= 0;
  m_NumberofCodedPFrame = 0;
  m_NumberofGOP			= 0;
  // remaining # of bits in GOP
  m_R					= 0;

  m_GAMMAP	= 0.5f;
  m_BETAP	= 0.5f;  
  
  // quadratic rate-distortion model  
  m_Pm_X1	= m_bit_rate * 1.0f;
  m_Pm_X2	= 0;

  // linear prediction model for P picture
  m_PSATDPictureC1 = 1.0f;
  m_PSATDPictureC2 = 0.0f;
  
  for( i = 0 ; i < MAX_SAMPLE ; i++ )
  {
	m_rgQp[ i ]					= 0.0f;
    m_rgRp[ i ]					= 0.0f;
	m_rcp_m_rgQp[ i ]			= 0.0f;			// 1.0f / m_rgQp[ i ]
	m_rcp_m_rgQp_m_rgQp[ i ]	= 0.0f;			// 1.0f / ( m_rgQp[ i ] * m_rgQp[ i ] )
	m_m_rgQp_m_rgRp[ i ]		= 0.0f;	

	m_PPictureSATD[ i ]			= 0;
  }
  m_PPictureSATD[ MAX_SAMPLE ]  = 0;

  RC_a00	= 0.0f;	RC_a01	 = 0.0f;  RC_a10   = 0.0f;	 RC_a11   = 0.0f;  RC_b0   = 0.0f;	RC_b1   = 0.0f;
  SATD_a00	= 0.0f; SATD_a01 = 0.0f;  SATD_a10 = 0.0f;	 SATD_a11 = 0.0f;  SATD_b0 = 0.0f;	SATD_b1 = 0.0f;
   
  // Define the largest variation of quantization parameters
  m_PDuantQp	 = 2;
  #if 0
  m_RC_MAX_QUANT = 51;  // clampping
  m_RC_MIN_QUANT = 0;	// clampping
  #endif
  // compute the initial QP
  bpp = m_bit_rate / ( m_frame_rate * seq->width * seq->height );
  if( seq->width == 176 )
  {
    L1 = 0.1f;
    L2 = 0.3f;
    L3 = 0.6f;
  }
  else if( seq->width == 352 )
  {
    L1 = 0.2f;
    L2 = 0.6f;
    L3 = 1.2f;
  }
  else
  {
    L1 = 0.6f;
    L2 = 1.4f;
    L3 = 2.4f;
  }
   
  if ( seq->initialQP == 0 )
  {
    if(bpp <= L1)
      qp = 35;
    else
      if(bpp <= L2)
        qp = 25;
      else
        if(bpp <= L3)
          qp  = 20;
        else
          qp = 10;
        seq->initialQP = qp;
  }
  
}


//----------------------------------------
//			rc_init_GOP
//	initialize parameter per-GOP
//	Richard	03/07/2006 
//
//----------------------------------------
void 
rc_init_GOP( int np, SEQUENCE* seq )
{
  bool  Overum = false;
  int   OverBits;
  int   OverDuantQp;
  int   AllocatedBits;
  int   GOPDquant;
  float temp;
  
  // check if the last GOP over uses its budget. If yes, the initial QP of the I frame in 
  // the coming  GOP will be increased.
  if( m_R < 0 )
    Overum = true;
  OverBits = -m_R;

  temp = m_bit_rate * m_rcp_frame_rate;

  // initialize the lower bound and the upper bound for the target bits of each frame, HRD consideration*/
  m_LowerBound  = (int)( m_R + temp );
  m_UpperBound1 = (int)( m_R + m_InitialDelayOffset );

  // compute the total number of bits for the current GOP
  AllocatedBits = (int)( (1 + np) * temp );
  m_R += AllocatedBits;
  m_Np = np;

  OverDuantQp = (int)( (OverBits << 3) / AllocatedBits );
  
  // Compute InitialQp for each GOP
  m_TotalPFrame = np;
  m_NumberofGOP++;
  if( m_NumberofGOP == 1 )
  {
    m_MyInitialQp = seq->initialQP;
    m_PreviousQp2 = m_MyInitialQp - 1; //recent change -0;
    QPLastGOP     = m_MyInitialQp;
  }
  else
  {    
    // compute the average QP of P frames in the previous GOP
   //m_PAverageQp = ( m_TotalQpforPPicture / m_NumberofPPicture );
   m_PAverageQp = (int)( 1.0*m_TotalQpforPPicture / m_NumberofPPicture+0.5 );    // fixed IP interval =1 issue


    GOPDquant = ( ( np + 1 ) / 15 );
    if( GOPDquant > 2 )
        GOPDquant = 2;

    m_PAverageQp -= GOPDquant;

    if ( m_PAverageQp > ( QPLastPFrame - 2 ) )
      m_PAverageQp--;
    m_PAverageQp = MAX( QPLastGOP - 2,  m_PAverageQp );
    m_PAverageQp = MIN( QPLastGOP + 2,  m_PAverageQp );
    m_PAverageQp = MIN( m_RC_MAX_QUANT, m_PAverageQp );
    m_PAverageQp = MAX( m_RC_MIN_QUANT, m_PAverageQp );
  

    m_MyInitialQp = m_PAverageQp;
    QPLastGOP     = m_MyInitialQp;
    m_Pm_Qp       = m_PAverageQp;
    m_PreviousQp1 = m_PreviousQp2;
    m_PreviousQp2 = m_MyInitialQp-1;  
  }
  m_TotalQpforPPicture=0;
  m_NumberofPPicture=0;
  
}



//----------------------------------------
//			rc_init_pict
//	initialize parameter per-picture
//	Richard	03/07/2006 
//
//----------------------------------------
void
rc_init_pict( int img_type, float bit_rate  )
{
  // predefine a target buffer level for each frame
  // P-frame only
  
  // Richard 05-18-2006
	if( m_PreviousBit_Rate != bit_rate )
	{
		m_bit_rate = bit_rate;
        m_R +=(int)( ( m_bit_rate - m_PreviousBit_Rate ) * m_Np * m_rcp_frame_rate );
	}
	//
	
    if( img_type == P_SLICE )
    {
        // predefine the  target buffer level for each picture.
		if( m_NumberofPPicture == 1 )
		{
            m_TargetBufferLevel = m_CurrentBufferFullness;
            m_DeltaP = ( m_CurrentBufferFullness - m_GOPTargetBufferLevel ) / ( m_TotalPFrame - 1 );
            m_TargetBufferLevel -= m_DeltaP;
		}
		else if( m_NumberofPPicture > 1 )
            m_TargetBufferLevel -= m_DeltaP;

      	if( m_NumberofCodedPFrame > 0 )
		{
			m_T  = (int)( m_R / m_Np );
			
			m_T1 = (int)( m_bit_rate * m_rcp_frame_rate - ( ( m_CurrentBufferFullness - m_TargetBufferLevel) >> 1 ) );
			m_T1 = MAX( 0, m_T1 );
			m_T  = (int)( ( m_T >> 1 ) + ( m_T1 >> 1 ) );
		}
		
		// HRD consideration
		m_T = MAX( m_T, m_LowerBound);
        m_T = MIN( m_T, m_UpperBound2);
    }
    
    m_PreviousBit_Rate = m_bit_rate;
    //m_NumberofHeaderBits  = 0;
    //m_NumberofTextureBits = 0;
}



//----------------------------------------
//	  updateQuantizationParameter
//	initialize parameter per-picture
//	Richard	03/07/2006 
//
//----------------------------------------
int updateQuantizationParameter( int img_type)
{
    int p_frame_satd;
  int   m_Bits;  
  float dtmp, sqrt_dtmp;
  float CurrentFrameSATD_MUL_m_X1, CurrentFrameSATD_MUL_m_X2;
  float sqrt_dtmp_SUB_CurrentFrameSATD_MUL_m_X1;
  unsigned int A;
  int f_adjust_DeltaQ=0;
  
  p_frame_satd=m_frame_cost;
  
  if( img_type == I_SLICE )
  {
	  m_Qc = m_MyInitialQp;	  
	  f_adjust_DeltaQ=0;
	  return m_Qc;
  }
  else if( ( img_type == P_SLICE ) && ( m_NumberofPPicture == 0 ) )
  {
        f_adjust_DeltaQ=check_satd_distribution(p_frame_satd);
	  m_Qc					= m_MyInitialQp;		  
	  
	  m_TotalQpforPPicture += m_Qc;
	  m_PreviousQp1			= m_PreviousQp2;
	  m_PreviousQp2			= m_Qc;
	  m_Pm_Qp				= m_Qc;
	  return m_Qc;  
  }
  else
  {
	  m_X1		= m_Pm_X1;
	  m_X2		= m_Pm_X2;
	  m_Qp		= m_Pm_Qp;
	  //m_DuantQp = m_PDuantQp;

   

	  
	  m_SATDPictureC1		= m_PSATDPictureC1;
	  m_SATDPictureC2		= m_PSATDPictureC2;
	  m_PreviousPictureSATD = m_PPictureSATD[ 0 ];
	  
	  // predict the SATD of current picture
	  m_CurrentFrameSATD = (float)( m_SATDPictureC1 * m_PreviousPictureSATD + m_SATDPictureC2 );
	  
	  // compute the number of bits for the texture	  
	  if( m_T < 0 )
	  {
		  m_Qc = m_Qp + m_DuantQp;
		  m_Qc = MIN( m_Qc, m_RC_MAX_QUANT ); // clipping
	  }
	  else
	  {
		  m_Bits = m_T;
		  m_Bits = MAX( m_Bits, (int)( m_bit_rate / ( 4.0f * m_frame_rate) ) );

		  CurrentFrameSATD_MUL_m_X1 = (float)m_CurrentFrameSATD * m_X1; 
		  CurrentFrameSATD_MUL_m_X2 = (float)m_CurrentFrameSATD * m_X2;
			
		  dtmp  = ( CurrentFrameSATD_MUL_m_X1 * CurrentFrameSATD_MUL_m_X1 );
		  dtmp += ( 4.0f * CurrentFrameSATD_MUL_m_X2 * (float)m_Bits );

		  A			= *(unsigned int*)&dtmp;
		  A			= ( A >> 1 ) + 0x1fc00000;
		  sqrt_dtmp = *(float*)&A;

		  sqrt_dtmp_SUB_CurrentFrameSATD_MUL_m_X1 = sqrt_dtmp - CurrentFrameSATD_MUL_m_X1;
		  	  
		  if( ( (*(unsigned int*)&m_X2) == 0x0 ) || ( dtmp < 0 ) || ( sqrt_dtmp_SUB_CurrentFrameSATD_MUL_m_X1 <= 0.0 ) )
			  m_Qstep = (float)( CurrentFrameSATD_MUL_m_X1 / m_Bits );
		  else // 2nd order mode
			  m_Qstep = (float)( (2 * CurrentFrameSATD_MUL_m_X2) / sqrt_dtmp_SUB_CurrentFrameSATD_MUL_m_X1 );

		  m_Qc = Qstep2QP( m_Qstep );

	      f_adjust_DeltaQ=check_satd_distribution(p_frame_satd);
		  
		  if(m_Qp<20)
          {
           m_DuantQp = m_PDuantQp<<3;
          }
          else if((m_Qp>=20)&&(m_Qp<30))
          {
           m_DuantQp = m_PDuantQp<<2;
          }
          else if((m_Qp>=30)&&(m_Qp<40))
          {
           m_DuantQp = m_PDuantQp<<1;
          }
          else if((m_Qp>=40)&&(m_Qp<45))
          {
           m_DuantQp = m_PDuantQp;
          }
          else //(m_Qp>45)
          {
           m_DuantQp = m_PDuantQp>>1;
          }
		  m_Qc = MIN( m_Qp + m_DuantQp,  m_Qc );				// control variation
		  m_Qc = MIN( m_Qc,				 m_RC_MAX_QUANT );		// clipping
		  
		  m_DuantQp = m_PDuantQp;
		  m_Qc = MAX( m_Qp - m_DuantQp,  m_Qc );				// control variation
		  m_Qc = MAX( m_RC_MIN_QUANT,    m_Qc );

		  if(f_adjust_DeltaQ)
		  {
		  	if(m_Qc<limited_RC_Min_Quant)
		  	{
		  		m_Qc=limited_RC_Min_Quant;
		  	}
		  }
		  
		  if(p_frame_satd<0)
	  	{
	  		m_Qc= m_MyInitialQp;
	  	}
	  }		  
	  
	  m_TotalQpforPPicture += m_Qc;
	  m_PreviousQp1			= m_PreviousQp2;
	  m_PreviousQp2			= m_Qc;
	  m_Pm_Qp				= m_Qc;
	  
	  return m_Qc;
  }
}

//----------------------------------------
//	     rc_update_pict_frame
//	update the complexity weight of I, P frame
//	Richard	03/07/2006 
//
//----------------------------------------
void 
rc_update_pict_frame( int img_type )
{
	if( img_type == P_SLICE )
	{
		m_Np--;
		m_NumberofCodedPFrame++;
		m_NumberofPPicture++;
	}
}


//----------------------------------------
//		rc_update_pict
//	
//	Richard	03/07/2006 
//
//----------------------------------------
void 
rc_update_pict( int nbits )
{
  int temp;

  temp = nbits - (int)( m_bit_rate * m_rcp_frame_rate);

  m_R -= nbits; // remaining # of bits in GOP
  m_CurrentBufferFullness += temp;	//nbits - m_bit_rate * m_rcp_frame_rate;

  //update the lower bound and the upper bound for the target bits of each frame, HRD consideration
  m_LowerBound  += (-temp); //( m_bit_rate * m_rcp_frame_rate - nbits );
  m_UpperBound1 += (-temp); //( m_bit_rate * m_rcp_frame_rate - nbits );
  m_UpperBound2  = (int)( m_OMEGA * m_UpperBound1 );
}



//----------------------------------------
//		updateRCModel
//	update R-C model parameter
//	Richard	03/15/2006 
//
//----------------------------------------
void 
updateRCModel( float current_frame_SATD, int img_type, int bits )
{

  int n_windowSize;
  int i;
  int m_Nc;
  bool MADModelFlag = false;

  if( img_type == P_SLICE )
  {
	  m_CurrentFrameSATD = current_frame_SATD;
      m_Nc = m_NumberofCodedPFrame;
	  
	  if( m_Nc > 1 )
		  MADModelFlag = true;

	  RC_a01 -= m_rcp_m_rgQp[ MAX_SAMPLE - 1 ];
	  RC_a11 -= m_rcp_m_rgQp_m_rgQp[ MAX_SAMPLE -1 ];
	  RC_b0  -= m_m_rgQp_m_rgRp[ MAX_SAMPLE - 1 ];
	  RC_b1  -= m_rgRp[ MAX_SAMPLE - 1 ];
	  
	  for (i = MAX_SAMPLE - 1; i > 0; i--) 
	  {
		  // update the history
		  m_rgQp[ i ]		=	m_rgQp[ i - 1 ];
		  m_rgRp[ i ]		=	m_rgRp[ i - 1 ];

		  m_rcp_m_rgQp[ i ]			= m_rcp_m_rgQp[ i - 1 ];
		  m_rcp_m_rgQp_m_rgQp[ i ]	= m_rcp_m_rgQp_m_rgQp[ i - 1 ];
		  m_m_rgQp_m_rgRp[ i ]		= m_m_rgQp_m_rgRp[ i - 1 ];
	  }
	  m_rgQp[ 0 ]				= QP2Qstep( m_Qc );
	  //m_rgRp[ 0 ]				= (float)( (float)bits / m_CurrentFrameSATD);
	  m_rgRp[ 0 ]				= (float)( (float)bits / (m_CurrentFrameSATD + 0.005 ));
	  m_rcp_m_rgQp[ 0 ]			= 1.0f / m_rgQp[0];							// 1.0f / m_rgQp[ i ]
	  m_rcp_m_rgQp_m_rgQp[ 0 ]	= m_rcp_m_rgQp[ 0 ] * m_rcp_m_rgQp[ 0 ];	// 1.0f / ( m_rgQp[ i ] * m_rgQp[ i ] )
	  m_m_rgQp_m_rgRp[ 0 ]		= m_rgQp[ 0 ] * m_rgRp[ 0 ];				// ( m_rgQp[ i ] * m_rgRp[ i ] )

	  m_X1 = m_Pm_X1;
	  m_X2 = m_Pm_X2;

	  RC_a01 += m_rcp_m_rgQp[ 0 ];
	  RC_a10 = RC_a01;
	  RC_a11 += m_rcp_m_rgQp_m_rgQp[ 0 ];
	  RC_b0  += m_m_rgQp_m_rgRp[ 0 ];
	  RC_b1  += m_rgRp[ 0 ];
	  
	  // compute the size of window
	  // n_windowSize = ( m_CurrentFrameSATD > m_PreviousFrameSATD ) ? 
	  //				   (int)( ( m_PreviousFrameSATD * MAX_SAMPLE ) / m_CurrentFrameSATD  ):
	  //				   (int)( ( m_CurrentFrameSATD * MAX_SAMPLE )  / m_PreviousFrameSATD );

	  n_windowSize = MAX_SAMPLE;

	  n_windowSize = MAX( n_windowSize, 1 );
	  n_windowSize = MIN( n_windowSize, m_Nc );
	  n_windowSize = MIN( n_windowSize, m_windowSize + 1 );
	  n_windowSize = MIN( n_windowSize, MAX_SAMPLE );
	  
	  // update the previous window size
	  m_windowSize	= n_windowSize;
	  RC_a00		= (float)n_windowSize;

	  // initial RD model estimator
	  RCModelEstimator( n_windowSize, img_type );
	  
	  n_windowSize = m_windowSize;
	  
	  if( MADModelFlag )
		  updateSATDModel( img_type );
	  else if( img_type == P_SLICE )
		  m_PPictureSATD[ 0 ] = m_CurrentFrameSATD;
  } 
}


//----------------------------------------
//		RCModelEstimator
//	update R-C model parameter(linear regression)
//	Richard	03/15/2006 
//
//----------------------------------------
void 
RCModelEstimator ( int n_windowSize, int img_type )
{
	int n_realSize = n_windowSize;
	float MatrixValue, pos_MatrixValue;
	
	// default RD model estimation results
	//m_X1 = m_X2 = 0.0;  
			
	m_X1 = ( RC_b0 / RC_a00);
	m_X2 = 0;
	
    // solve the equation of AX = B
    MatrixValue = RC_a00 * RC_a11 - RC_a01 * RC_a10;
	pos_MatrixValue	= (float)fabs( MatrixValue );
	
    if( (n_realSize >= 1) && ( (*(unsigned int*)&pos_MatrixValue) > 0x358637bd ) )
    {
		// take 2nd order model to estimate X1 and X2    
		m_X1 = (float)( ( RC_b0 * RC_a11 - RC_b1 * RC_a01 ) / MatrixValue );
        m_X2 = (float)( ( RC_b1 * RC_a00 - RC_b0 * RC_a10 ) / MatrixValue );
    }
	if( img_type == P_SLICE )
	{
		m_Pm_X1 = m_X1;
		m_Pm_X2 = m_X2;
	}
}



//----------------------------------------
//		updateSATDModel
//	update SATD model parameter
//	Richard	03/15/2006 
//
//----------------------------------------
void 
updateSATDModel( int img_type )
{
  int n_windowSize;
  int i;
  int m_Nc;
  
  if( m_NumberofCodedPFrame > 0 )
  {
    m_Nc = m_NumberofCodedPFrame;
    
    for (i = MAX_SAMPLE - 1; i > 0; i--) 
    {
	  // update the history
	  m_PPictureSATD[ i ]  = m_PPictureSATD[ i - 1 ];
      m_PictureSATD[ i ]   = m_PPictureSATD[ i ];
      m_ReferenceSATD[ i ] = m_ReferenceSATD[ i - 1 ];
    }
	m_PPictureSATD[0] = m_CurrentFrameSATD;
    m_PictureSATD[0]  = m_PPictureSATD[ 0 ];

	m_ReferenceSATD[ 0 ] = m_PictureSATD[ 1 ];

	m_SATDPictureC1 = m_PSATDPictureC1;
    m_SATDPictureC2 = m_PSATDPictureC2;
    
	//n_windowSize = ( m_CurrentFrameSATD > m_PreviousFrameSATD ) ?
	//				(int)( ( m_PreviousFrameSATD * MAX_SAMPLE ) / m_CurrentFrameSATD  ) :
	//				(int)( ( m_CurrentFrameSATD  * MAX_SAMPLE ) / m_PreviousFrameSATD );
	n_windowSize = MAX_SAMPLE;
    
    n_windowSize = MIN( n_windowSize, (m_Nc - 1) );
    n_windowSize = MAX( n_windowSize, 1);
    n_windowSize = MIN( n_windowSize, m_SATDm_windowSize + 1 );
    n_windowSize = MIN( MAX_SAMPLE, n_windowSize);

    // update the previous window size
    m_SATDm_windowSize = n_windowSize;
    
    // update the SATD for the previous frame
    if( img_type == P_SLICE )
		m_PreviousFrameSATD = m_CurrentFrameSATD;

	SATDModelEstimator( n_windowSize, img_type );
  }
}



//----------------------------------------
//		SATDModelEstimator
//	update SATD model parameter(linear regression)
//	Richard	03/15/2006 
//
//----------------------------------------
void 
SATDModelEstimator( int n_windowSize, int img_type )
{
	int n_realSize = n_windowSize;
	int i;
	float a00 = 0.0, a01 = 0.0, a10 = 0.0, a11 = 0.0, b0 = 0.0, b1 = 0.0;
	float MatrixValue, pos_MatrixValue;;
	
	// default SATD model estimation results  
	m_SATDPictureC1 = m_SATDPictureC2 = 0.0;
	
	// take 2nd order model to estimate X1 and X2
	for (i = 0; i < n_windowSize; i++) 
	{
		a00  = a00 + 1.0f;
		a01 += m_ReferenceSATD[ i ];
		a10  = a01;
		a11 += m_ReferenceSATD[ i ] * m_ReferenceSATD[ i ];
		b0  += m_PictureSATD[ i ];
		b1  += m_PictureSATD[ i ] * m_ReferenceSATD[ i ];      
	}
    m_SATDPictureC1 = b0 / a01;
    m_SATDPictureC2 = 0.0;
	
    MatrixValue		= a00 * a11 - a01 * a10;
	pos_MatrixValue = (float)fabs( MatrixValue );

    if( (n_realSize >= 1) && ( (*(unsigned int*)&pos_MatrixValue) > 0x358637bd ) ) 
    {
		m_SATDPictureC2 = ( b0 * a11 - b1 * a01 ) / MatrixValue;
		m_SATDPictureC1 = ( b1 * a00 - b0 * a10 ) / MatrixValue;
    }
	
	if( img_type == P_SLICE )
	{
		m_PSATDPictureC1 = m_SATDPictureC1;
		m_PSATDPictureC2 = m_SATDPictureC2;
	}
}


//----------------------------------------
//			Qstep2QP
//	a fast Qstep to Qp solution
//		Richard	03/12/2006 
//
//----------------------------------------
int 
Qstep2QP( float Qstep )
{
  int			q_per = 0, q_rem = 0;
  unsigned int	temp, E, MAN;
  
  if( *(unsigned int*)&Qstep < 0x3f800000 )
    return 0;
  else if ( *(unsigned int*)&Qstep > 0x43740000 )
    return 51;
  
  temp	= *(unsigned int*)&Qstep;
  E		= ( temp >> 23 ) & 0xff;
  MAN	= ( temp & 0x7fffff );

  if( temp > 0x3f800000 )
  {	  
	  q_per  = ( MAN == 0x0 ) ? ( E - 127 ) : ( E - 126 );
	  E		-= q_per;
  }
  
 if( E == 0x7e )
  {
	  if( MAN <= 0x00280000 )
		  q_rem = 0;
	  else if ( MAN <= 0x00400000 )
		  q_rem = 1;
	  else if( MAN <= 0x00580000 )
		  q_rem = 2;
	  else if( MAN <= 0x00700000 )
		  q_rem = 3;
	  else
	  	 q_rem = 4;
  }
  else 
  {   
      if ( MAN <= 0x00080000 )
	  	q_rem = 4;
	  else 
	  	q_rem = 5;
  } 
  
  return( q_per * 6 + q_rem );
}



//----------------------------------------
//			QP2Qstep
//	a fast QP to Qstep solution
//		Richard	03/12/2006 
//
//----------------------------------------
float 
QP2Qstep( int QP )
{
  static const float QP2QSTEP[ 52 ] = 
			{ 
			1.0,   1.0,  1.0,  1.0,  1.0,   1.0,   1.0,   1.0,   2.0,   2.0,
			2.0,   2.0,  2.0,  2.0,  3.0,   3.0,   4.0,   4.0,   5.0,   5.0,
			6.0,   7.0,  8.0,  9.0,  10.0,  11.0,  13.0,  14.0,  16.0,  18.0,
			20.0,  22.0, 26.0, 28.0, 32.0,  36.0,  40.0,  44.0,  52.0,  56.0,
			64.0,  72.0, 80.0, 88.0, 104.0, 112.0, 128.0, 144.0, 160.0, 176.0,
			208.0, 244.0 };

  return QP2QSTEP[ QP ];
}


void H264RateControlInit(SEQUENCE *_seq,float br,float frame_rate,int width,int height,int u32IPInterval,int qmax,int qmin, int initqp)
{
    _seq->jumpd      = 0;
    _seq->bit_rate   = br;
    _seq->frame_rate = frame_rate;
    _seq->width      = width;
    _seq->height     = height;
    _seq->initialQP  = initqp;
    _seq->u32IPInterval=u32IPInterval;
    _seq->interval_count =0;
    m_RC_MAX_QUANT=qmax;
    m_RC_MIN_QUANT=qmin;
    rc_init_seq(_seq);
}


int H264RateControlGetQuant(SEQUENCE *_seq)
{
    if(_seq->bit_rate)
    {
        if((_seq->interval_count==0) ||
           ((_seq->u32IPInterval>0)&&(_seq->interval_count>=_seq->u32IPInterval)) )
        {
            _seq->img_type = I_SLICE;		    
            rc_init_GOP(_seq->u32IPInterval-1, _seq);
        }
        else
            _seq->img_type = P_SLICE;
    
        rc_init_pict(_seq->img_type,_seq->bit_rate);
        return updateQuantizationParameter(_seq->img_type);
    }
    return 0;
}


void H264RateControlUpdate(SEQUENCE *_seq,int bitstream_size,int frame_cost)
{
    float avg_current_frame_SATD;

    //frame_cost += rtcl_offset;
    //if(frame_cost<0)
	//frame_cost = 0;			

    // if bitrate is zero, then don't invoke rate control 
    if(_seq->bit_rate) 
    {
        rc_update_pict_frame(_seq->img_type); //rc_update_pict_frame( bitstream_size, img_type );
        rc_update_pict(bitstream_size<<3);
        //avg_current_frame_SATD = ( frame_cost/256.0 ) / ((_seq->width>>4)*(_seq->height>>4));
        m_frame_cost = avg_current_frame_SATD = frame_cost;
        updateRCModel( avg_current_frame_SATD, _seq->img_type, (bitstream_size<<3) * 0.6 );

        _seq->interval_count++;
        if(_seq->u32IPInterval>0)
            _seq->interval_count %= _seq->u32IPInterval;
    }
}



int check_satd_distribution(int satd)
{
	int check_frameNum=0;
	int *pSatd;
	int ii;
	int result=0;
	
	check_frameNum=((int)m_frame_rate)/check_range;
	
	//interation previous satd
	pSatd=satd_array;

	for(ii=check_frameNum+1;ii>=1;ii--)
	{
		//*(pSatd+ii)=*(pSatd+ii-1);
		*(pSatd+ii)=*(pSatd+ii-1);
	}
	*pSatd=satd;
	
	result=0;

	for(ii=check_frameNum;ii>=0;ii--)
	{
		int temp=*(pSatd+ii);

		if(temp<0)
		{
			result=1;
			break;
		}
	}
	return result;
}